import numpy as np 

stateTrajectory_closedLoop_mz1 = np.loadtxt('../results/StateCL_mz1_fs_lmz_14_mad65.txt')
stateTrajectory_closedLoop_mz1 = np.loadtxt('../results/StateCL_mz1_fs_lmz_14_mad65.txt')
stateTrajectory_closedLoop_mz2 = np.loadtxt('../results/StateCL_mz2_fs_lmz_14_mad65.txt')
stateTrajectory_closedLoop_mz3 = np.loadtxt('../results/StateCL_mz3_fs_lmz_14_mad65.txt')

u_prescribed_mz1 = -1*np.loadtxt('../results/InputCL_mz1_fs_lmz_14_mad65.txt')
u_prescribed_mz2 = -1*np.loadtxt('../results/InputCL_mz2_fs_lmz_14_mad65.txt')
u_prescribed_mz3 = -1*np.loadtxt('../results/InputCL_mz3_fs_lmz_14_mad65.txt')

lower_bound_violation_mz1 = 0.176*np.ones(len(stateTrajectory_closedLoop_mz1)) - stateTrajectory_closedLoop_mz1 
lower_bound_violation_mz2 = 0.176*np.ones(len(stateTrajectory_closedLoop_mz2)) - stateTrajectory_closedLoop_mz2
lower_bound_violation_mz3 = 0.209*np.ones(len(stateTrajectory_closedLoop_mz3)) - stateTrajectory_closedLoop_mz3

upper_bound_violation_mz1 = stateTrajectory_closedLoop_mz1 - 0.28*np.ones(len(stateTrajectory_closedLoop_mz1))
upper_bound_violation_mz2 = stateTrajectory_closedLoop_mz2 - 0.28*np.ones(len(stateTrajectory_closedLoop_mz2))
upper_bound_violation_mz3 = stateTrajectory_closedLoop_mz3 - 0.30*np.ones(len(stateTrajectory_closedLoop_mz3))

lower_bound_violation_mz1[lower_bound_violation_mz1<0]=0
lower_bound_violation_mz2[lower_bound_violation_mz2<0]=0
lower_bound_violation_mz3[lower_bound_violation_mz3<0]=0

upper_bound_violation_mz1[upper_bound_violation_mz1<0]=0
upper_bound_violation_mz2[upper_bound_violation_mz2<0]=0
upper_bound_violation_mz3[upper_bound_violation_mz3<0]=0

lower_bound_violation_mz1 = lower_bound_violation_mz1**2
lower_bound_violation_mz2 = lower_bound_violation_mz2**2
lower_bound_violation_mz3 = lower_bound_violation_mz3**2

upper_bound_violation_mz1 = upper_bound_violation_mz1**2
upper_bound_violation_mz2 = upper_bound_violation_mz2**2
upper_bound_violation_mz3 = upper_bound_violation_mz3**2

cost_lower_bound_violation_mz1 = 2.0e07*sum(lower_bound_violation_mz1)
cost_lower_bound_violation_mz2 = 2.0e07*sum(lower_bound_violation_mz2)
cost_lower_bound_violation_mz3 = 2.0e07*sum(lower_bound_violation_mz3)

cost_upper_bound_violation_mz1 = 2.2e07*sum(upper_bound_violation_mz1)
cost_upper_bound_violation_mz2 = 2.2e07*sum(upper_bound_violation_mz2)
cost_upper_bound_violation_mz3 = 2.2e07*sum(upper_bound_violation_mz3)


cost_rotations_mz1 = 10000
cost_rotations_mz2 = 10000
cost_rotations_mz3 = 10000

cost_irrigations_mz1 = 9*sum(u_prescribed_mz1)
cost_irrigations_mz2 = 9*sum(u_prescribed_mz2)
cost_irrigations_mz3 = 9*sum(u_prescribed_mz3)

total_irrigation_rate = sum(u_prescribed_mz1) + sum(u_prescribed_mz2) + sum(u_prescribed_mz3)


total_irrig_cost_quadrant = cost_irrigations_mz1+cost_irrigations_mz2+cost_irrigations_mz3

total_cost_upper_bound_violation = cost_upper_bound_violation_mz1 + cost_upper_bound_violation_mz2 + cost_upper_bound_violation_mz3
total_cost_lower_bound_violation = cost_lower_bound_violation_mz1 + cost_lower_bound_violation_mz2 + cost_lower_bound_violation_mz3

overall_cost = 10000+total_irrig_cost_quadrant  + total_cost_lower_bound_violation + total_cost_upper_bound_violation

overall_cost_mz1 = cost_lower_bound_violation_mz1 + cost_upper_bound_violation_mz1 + cost_rotations_mz1 + cost_irrigations_mz1
overall_cost_mz2 = cost_lower_bound_violation_mz2 + cost_upper_bound_violation_mz2 + cost_rotations_mz2 + cost_irrigations_mz2
overall_cost_mz3 = cost_lower_bound_violation_mz3 + cost_upper_bound_violation_mz3 + cost_rotations_mz3 + cost_irrigations_mz3
